#pragma ident "$Id: ClockConstraintForPCE.cpp $"

/**
 * @file ClockConstraintForPCE.cpp
 * Extended Kalman Filter process of ClockConstraint for PCE.
 */

#include "ClockConstraintForPCE.hpp"
#include "Counter.hpp"

#ifdef USE_OPENMP
#include <omp.h>
#endif

using namespace std;

namespace gpstk
{

    // Return a string identifying this object.
    std::string ClockConstraintForPCE::getClassName() const
    { return "ClockConstraintForPCE"; }


    /** Return a reference to a sourceRinex object after solving
     *  the previously defined equation system.
     *
     * @param gData    Data object holding the data.
     */
    void ClockConstraintForPCE::Process( commonHeader& comHeader, sourceRinex& gData )
        noexcept(false)
    {

//        cout << "ClockConstraintForPCE::Process()" << endl;

        double clk1( Counter::now() );

        try
        {
            time = pSolStore->getCurrentTime();

            numUnknown = pSolStore->getNumUnknown();

            sourceSet = pSolStore->getSourceIDSet();
            satSet = pSolStore->getSatIDSet();

            state = pSolStore->getStateVector();
            covar = pSolStore->getCovarMatrix();

            sourceIndex = pSolStore->getSourceIndex();
            satIndex = pSolStore->getSatIndex();
            ambiIndex = pSolStore->getAmbiIndex();

            int idSource(0), idSat(0), idAmbi(0);

            double cdtSta(0.0), zwd(0.0);
            double cdtSat(0.0), cdtSatDot(0.0);
            double ambi(0.0);

            double omc(0.0), weight(0.0), wmf(0.0);

            Vector<double> h(numUnknown,0.0);
            Vector<double> pht(numUnknown,0.0);
            double hpht(0.0);
            double beta(0.0);
            Vector<double> gamma(numUnknown,0.0);


            //////// clock constraint ////////
            satValueMap satClock( pSolStore->getSatClock() );
            double clock(0.0);

            double obs(0.0), com(0.0);
            for(SatIntMap::iterator itSat = satIndex.begin();
                itSat != satIndex.end();
                ++itSat)
            {
                SatID sat( itSat->first );
                clock = satClock[sat];

                idSat = itSat->second;

                obs += clock;
                com += state[idSat+0];

                h(idSat) = 1.0;
            }

            omc = obs - com;

            weight = 2.0;

            for(int i=0; i<numUnknown; i++)
            {
                for(int j=0; j<numUnknown; j++)
                {
                    if(h(j) != 0.0)
                        pht(i) += covar(i,j) * h(j);
                }
            }

            for(int i=0; i<numUnknown; i++)
            {
                if(h(i) != 0.0)
                    hpht += h(i) * pht(i);
            }

            beta = 1.0/weight + hpht;

            gamma = pht/beta;

            // state update
            state = state + gamma*omc;

            // covar update
#ifdef USE_OPENMP
    #pragma omp parallel for
#endif
            for(int i=0; i<numUnknown; i++)
            {
                covar(i,i) = covar(i,i) - gamma(i)*pht(i);

                for(int j=i+1; j<numUnknown; j++)
                {
                    covar(i,j) = covar(i,j) - gamma(i)*pht(j);
                    covar(j,i) = covar(i,j);
                }
            }

            pSolStore->setCurrentTime( time );
            pSolStore->setStateVector( state );
            pSolStore->setCovarMatrix( covar );

        }
        catch(Exception& u)
        {
            // Throw an exception if something unexpected happens
            ProcessingException e( getClassName() + ":" + u.what() );
            GPSTK_THROW(e);
        }

        double clk2( Counter::now() );

//        cout << "time ellapsed: " << clk2-clk1 << endl;

    }  // End of method 'ClockConstraintForPCE::Process()'


}  // End of namespace gpstk
